/**
 * @file mb_slave.c
 * @author your name (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2020-08-25
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include "mb_slave.h"
#include "3rd/libmodbus/modbus.h"
#include "config.h"
#include "modbus_io_mgr.h"
#include "log/zuclog.h"
#include "log/zlog.hh"
#include "jkutil/iniparser.h"

#include "zuc/nml_intf/zucpos.h"
#define NB_CONNECTION 5
/* MACRO */
#define SERVER_ID 1
#define INVALID_SERVER_ID 18

#define QUERY_POS_SLAVE_ID_BYTE (0)
#define QUERY_POS_CMD_CODE_BYTE (1)
#define QUERY_POS_ADDR_H_BYTE (2)
#define QUERY_POS_ADDR_L_BYTE (3)
#define QUERY_POS_LEN_H_BYTE (4)
#define QUERY_POS_LEN_L_BYTE (5)

#define MODBUS_QUERY_RTU_OFFSET (0)
#define MODBUS_QUERY_TCP_OFFSET (6)
#define MODBUS_QUERY_ID_INDEX (0)
#define MODBUS_QUERY_FC_INDEX (1)
#define MODBUS_QUERY_ADDR_H_INDEX (2)
#define MODBUS_QUERY_ADDR_L_INDEX (3)
#define MODBUS_QUERY_LEN_H_INDEX (4)
#define MODBUS_QUERY_LEN_L_INDEX (5)

#define QUERY_POS_WRITE_DATA_1_BYTE (6)
#define QUERY_POS_WRITE_DATA_2_BYTE (7)
#define QUERY_POS_WRITE_DATA_3_BYTE (8)
#define QUERY_POS_WRITE_DATA_4_BYTE (9)
#define QUERY_POS_WRITE_DATA_5_BYTE (10)
#define QUERY_POS_WRITE_DATA_6_BYTE (11)
#define QUERY_POS_WRITE_DATA_7_BYTE (12)
#define QUERY_POS_WRITE_DATA_8_BYTE (13)
#define QUERY_POS_WRITE_DATA_9_BYTE (14)
#define QUERY_POS_WRITE_DATA_10_BYTE (15)
#define QUERY_POS_WRITE_DATA_11_BYTE (16)

const uint16_t SERVER_BITS_ADDRESS = MB_COIL_GEN_0;               //40
const uint16_t SERVER_BITS_NB = MB_COIL_GEN_ALL - MB_COIL_GEN_0;  //

const uint16_t SERVER_INPUT_BITS_ADDRESS = MB_D_INPT_GEN_0;                 //8
const uint16_t SERVER_INPUT_BITS_NB = MB_D_INPT_GEN_ALL - MB_D_INPT_GEN_0;  //

const uint16_t SERVER_REGISTERS_ADDRESS = MB_HOLDING_GEN_0;              //100
const uint16_t SERVER_REGISTERS_NB = MB_HOLDING_ALL - MB_HOLDING_GEN_0;  //

const uint16_t SERVER_INPUT_REGISTERS_ADDRESS = MB_IN_REG_GEN_0;                                 //96
const uint16_t SERVER_INPUT_REGISTERS_NB = (uint16_t)MB_IN_REG_ALL - (uint16_t)MB_IN_REG_GEN_0;  //288-96= 192

const float SERVER_REAL = 916.540649;
const uint32_t SERVER_IREAL = 0x4465229a;

/* LOCAL VARIABLES */
static modbus_t* ctx;
static int s = -1;
static uint8_t* query;
static int header_length;
static int server_socket = -1;
mb_slave_t mb_slave;

void make_nonblocking(int fd) { fcntl(fd, F_SETFL, O_NONBLOCK); }

static void close_sigint(int code)
{
    if (server_socket != -1)
    {
        zucInnerDbg("[Modbus-Slave]:>>TCP close server_socket!\n");
        zucLogError(0, "[Modbus-Slave]:>>TCP close server_socket!");
        close(server_socket);
    }

    modbus_free(ctx);
    modbus_mapping_free(mb_slave.slave_reg_mapping);
    zucInnerDbg("[Modbus-Slave]:>>TCP quit !\n");
    zucLogError(0, "[Modbus-Slave]:>>TCP quit !");
    exit(code);
}

static int mb_slave_UpdateLocalData(void);
static void mbsrv2hal_getModbusSlavePara(mb_slave_comm_para_t& slaveCommuConfig, int cabType);

/**
 * @brief 
 * 	update the data for command 0x01
 */
char mbsrv2hal_update_pinstatus_readcoils(unsigned short coils_address, unsigned short coils_cnt)
{
    int i;

    if (coils_address < MB_SRV_COILS_START_ADDRESS || (coils_address + coils_cnt > MB_SRV_COILS_START_ADDRESS + MB_SRV_COILS_LENGTH))
    {
        return -1;
    }
    else
    {
        for (i = coils_address; i < coils_cnt + coils_address; i++)
        {
            mb_slave.slave_reg_mapping->tab_bits[i] = ModbusIOShmStruct->MbSlaveShmStruct.coils[i];
            //read physic do
            if (i > MB_COIL_GEN_127)
            {
                mb_slave.slave_reg_mapping->tab_bits[i] = ModbusPhysicIOStruct->physic_do[i - MB_COIL_GEN_PHY_DO_1];
            }
        }
        return 0;
    }
}
/**
 * @brief 
 *	update the data for command 0x02
 * 	
 * @param coils_address 
 * @param coils_cnt 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_readDiscretInputs(unsigned short coils_address, unsigned short coils_cnt)
{
    int i;
    if (coils_cnt + coils_address > MB_SRV_INPUT_REGISTER_START_ADDRESS + MB_SRV_DISCRETE_INPUT_LENGTH)
    {
        return -1;
    }
    else
    {
        for (i = coils_address; i < coils_cnt + coils_address; i++)
        {
            mb_slave.slave_reg_mapping->tab_input_bits[i] = ModbusIOShmStruct->MbSlaveShmStruct.discrete_input[i];
        }
    }
    return 0;
}
/**
 * @brief 
 *	update the data for command 0x03
 * 
 * @param registers_address 
 * @param registers_cnt 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_readHoldingRegisters(unsigned short registers_address, unsigned short registers_cnt)
{
    int i;
    if (registers_address < MB_SRV_HOLDING_REGISTER_START_ADDRESS ||
        (registers_address + registers_cnt > MB_SRV_HOLDING_REGISTER_LENGTH + MB_SRV_HOLDING_REGISTER_START_ADDRESS))
    {
        return -1;
    }
    else
    {
        for (i = registers_address; i < registers_cnt + registers_address; i++)
        {
            mb_slave.slave_reg_mapping->tab_registers[i] = ModbusIOShmStruct->MbSlaveShmStruct.holding_reg[i];
            //read physic ao
            if (i > MB_HOLDING_FLOAT_31_L)
            {
                uint16_t tempData = ModbusPhysicIOStruct->physic_cab_ao.data[i - MB_HOLDING_PHY_AO_1];
                mb_slave.slave_reg_mapping->tab_registers[i] = (tempData >> 8) + (tempData << 8);
            }
        }
        return 0;
    }
}
/**
 * @brief 
 * update the data for command 0x04
 * @param registers_address 
 * @param registers_cnt 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_readInputRegisters(unsigned short registers_address, unsigned short registers_cnt)
{
    int i;
    if (registers_address < MB_SRV_INPUT_REGISTER_START_ADDRESS ||
        (registers_address + registers_cnt > (uint32_t)MB_SRV_INPUT_REGISTER_START_ADDRESS + (uint32_t)MB_SRV_INPUT_REGISTER_LENGTH))
    {
        return 0;
    }
    else
    {
        for (i = registers_address; i < registers_cnt + registers_address; i++)
        {
            mb_slave.slave_reg_mapping->tab_input_registers[i] = (unsigned short)(ModbusIOShmStruct->MbSlaveShmStruct.input_reg[i]);
        }
        return 0;
    }
}
/**
 * @brief 
 * update the data for command 0x05
 * 
 * @param coils_address 
 * @param data_bytes 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_WriteSinglcoil(unsigned short coils_address, unsigned short data_bytes)
{
    if (coils_address < MB_SRV_COILS_START_ADDRESS || (coils_address > MB_SRV_COILS_START_ADDRESS + MB_SRV_COILS_LENGTH))
    {
        return -1;
    }
    else
    {
        if (data_bytes == 0xFF00)
        {
            ModbusIOShmStruct->MbSlaveShmStruct.coils[coils_address] = 1;
            if (coils_address > MB_COIL_GEN_127)
            {
                ModbusPhysicIOStruct->physic_do[coils_address - MB_COIL_GEN_PHY_DO_1] = 1;
            }
        }
        else if (data_bytes == 0x00)
        {
            ModbusIOShmStruct->MbSlaveShmStruct.coils[coils_address] = 0;
            if (coils_address > MB_COIL_GEN_127)
            {
                ModbusPhysicIOStruct->physic_do[coils_address - MB_COIL_GEN_PHY_DO_1] = 0;
            }
        }
        else if (data_bytes == 0xFFFF)
        {
            /* release ???*/
            /* Nothing tobe Done*/
        }
    }
    return 0;
}
/**
 * @brief update the data for command 0x06
 * 
 * @param registers_address 
 * @param data_bytes 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_WriteSinglRegister(unsigned short registers_address, unsigned short data_bytes)
{
    /* update slave ID */
    if (registers_address < MB_SRV_HOLDING_REGISTER_START_ADDRESS ||
        (registers_address > MB_SRV_HOLDING_REGISTER_START_ADDRESS + MB_SRV_HOLDING_REGISTER_LENGTH))
    {
        return -1;
    }
    else
    {
        ModbusIOShmStruct->MbSlaveShmStruct.holding_reg[registers_address] = data_bytes;
        if (registers_address > MB_HOLDING_FLOAT_31_L)
        {
            ModbusPhysicIOStruct->physic_cab_ao.data[registers_address - MB_HOLDING_PHY_AO_1] = (data_bytes << 8) + (data_bytes >> 8);
        }
    }
    return 0;
}
/**
 * @brief update the data for command 0x0F
 * 		coils are written by modbus master
 * 		the coils status feedback to motion module
 * 
 * 	PLC(modbus master) --> mb-slave --> motion
 * 
 * @param coils_address 
 * @param coils_cnt 
 * @param byte_length 
 * @param byte_ptr 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_Writecoils(unsigned short coils_address, unsigned short coils_cnt, char byte_length, unsigned char* byte_ptr)
{
    int i;
    if (coils_address < MB_SRV_COILS_START_ADDRESS || (coils_address + coils_cnt > MB_SRV_COILS_START_ADDRESS + MB_SRV_COILS_LENGTH))
    {
        return 0;
    }
    else
    {
        for (i = coils_address; i < coils_cnt + coils_address; i++)
        {
            ModbusIOShmStruct->MbSlaveShmStruct.coils[i] = (byte_ptr[(i - coils_address) / 8] & (1 << ((i - coils_address) % 8))) == 0 ? 0 : 1;
            if (i > MB_COIL_GEN_127)
            {
                ModbusPhysicIOStruct->physic_do[i - MB_COIL_GEN_PHY_DO_1] = (byte_ptr[(i - coils_address) / 8] & (1 << ((i - coils_address) % 8))) == 0 ? 0 : 1;
            }
        }
    }

    return 0;
}
/**
 * @brief update the data for command 0x10
 * 
 * @param registers_address 
 * @param registers_cnt 
 * @param byte_length 
 * @param byte_ptr 
 * @return char 
 */
char mbsrv2hal_update_pinstatus_WriteRegisters(unsigned short registers_address, unsigned short registers_cnt, char byte_length, unsigned char* byte_ptr)
{
    int i;

    if (registers_address < MB_SRV_HOLDING_REGISTER_START_ADDRESS ||
        (registers_address + registers_cnt > MB_SRV_HOLDING_REGISTER_START_ADDRESS + MB_SRV_HOLDING_REGISTER_LENGTH))
    {
        return 0;
    }
    else
    {
        for (i = registers_address; i < registers_address + registers_cnt; i++)
        {
            ModbusIOShmStruct->MbSlaveShmStruct.holding_reg[i] =
                (unsigned short)(byte_ptr[2 * (i - registers_address)] << 8) + (unsigned short)(byte_ptr[2 * (i - registers_address) + 1]);
            if (i > MB_HOLDING_FLOAT_31_L)
            {
                ModbusPhysicIOStruct->physic_cab_ao.data[i - MB_HOLDING_PHY_AO_1] =
                    (unsigned short)(byte_ptr[2 * (i - registers_address)] << 8) + (unsigned short)((byte_ptr[2 * (i - registers_address) + 1]) << 8);
            }
        }
    }

    return 0;
}
/**
 * @brief data mapping init
 * 
 */
//
static void mbsrv2hal_datamappingInit(void)
{
    int i;
    /* create the data mapping*/
    mb_slave.slave_reg_mapping = modbus_mapping_new(SERVER_BITS_ADDRESS + SERVER_BITS_NB,
                                                    SERVER_INPUT_BITS_ADDRESS + SERVER_INPUT_BITS_NB,
                                                    SERVER_REGISTERS_ADDRESS + SERVER_REGISTERS_NB,
                                                    SERVER_INPUT_REGISTERS_ADDRESS + SERVER_INPUT_REGISTERS_NB);
    if (mb_slave.slave_reg_mapping == NULL)
    {
        fprintf(stderr, "Failed to allocate the mapping: %s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return;
    }

    /** INPUT REGISTERS **/
    for (i = 0; i < SERVER_INPUT_REGISTERS_NB; i++) { mb_slave.slave_reg_mapping->tab_input_registers[SERVER_INPUT_REGISTERS_ADDRESS + i] = i; }
}

/* GLOBAL FUNCTIONS */
/**
 * @brief initialize TCP channel for modbus interface
 * 
 */
char mb_slave_tcp_init(void)
{
    /* init modbus parameters */
    ctx = modbus_new_tcp(NULL, mb_slave.slave_comm_para.tcp_comm_para.port, "");

    if (ctx == NULL)
    {
        zucInnerDbg("Open modbus TCP failed\n");
        return -1;
    }

    /* Set slave address */
    modbus_set_slave(ctx, mb_slave.slave_comm_para.slave_id);

    header_length = modbus_get_header_length(ctx);

    /* debug phase to set the debug option true */
    modbus_set_debug(ctx, FALSE);

    modbus_set_indication_timeout(ctx, 5, 0);
    modbus_set_response_timeout(ctx, 0, 100 * 1000);

    return 0;
}
/**
 * @brief init RTU channel for modbus-RTU interface
 * 
 * @param devPath 
 * @param baudrate 
 * @param parity 
 * @param stopBit 
 * @param dataBitLen 
 * @return char 
 */
char mb_slave_rtu_init(char* devPath, int baudrate, char parity, char stopBit, char dataBitLen)
{
    int rc = 0;
    /**/
    if (devPath == NULL)
    {
        return -1;
    }

    /* default parity NONE */
    if (parity != 'N' && parity != 'E' && parity != 'O')
    {
        parity = 'N';
    }
    /* default stopBit 1*/
    if (stopBit != 1 && stopBit != 2)
    {
        stopBit = 1;
    }
    /* default dataBitLen 8*/
    if (dataBitLen != 7 && dataBitLen != 8)
    {
        dataBitLen = 8;
    }
    /* init modbus parameters */
    ctx = modbus_new_rtu(devPath, baudrate, parity, dataBitLen, stopBit);
    if (ctx == NULL)
    {
        zucInnerDbg("Open modbus device failed\n");
        return -1;
    }
    /* Set slave address */
    modbus_set_slave(ctx, mb_slave.slave_comm_para.slave_id);

    /* Modbus_Application_Protocol_V1_1b.pdf 
	 * Chapter 4 Section 1 Page 5
	 * RS232/RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
	 */
    query = (uint8_t*)malloc(MODBUS_RTU_MAX_ADU_LENGTH);

    header_length = modbus_get_header_length(ctx);

    /* debug phase to set the debug option true */
    modbus_set_debug(ctx, FALSE);

    mbsrv2hal_datamappingInit();

    modbus_set_indication_timeout(ctx, 30, 0);
    modbus_set_response_timeout(ctx, 0, 100 * 1000);

    /* Connect Modebus*/
    rc = modbus_connect(ctx);
    if (rc == -1)
    {
        fprintf(stderr, "Unable to connect %s\n", modbus_strerror(errno));
        free(query);
        query = NULL;
        modbus_free(ctx);
        ctx = NULL;
        modbus_mapping_free(mb_slave.slave_reg_mapping);
        mb_slave.slave_reg_mapping = NULL;

        return -1;
    }
    return 0;
} /* End MBIF_modbusInit */

/**
 * @brief global init interface
 * 
 * @return int 
 */
int mb_slave_init(void)
{
    zucInnerDbg("[Modbus-Slave]:>>Start Init operation-->get configure parameter !");
    mbsrv2hal_getModbusSlavePara(mb_slave.slave_comm_para, ModbusIOShmStruct->MbSlaveShmStruct.zuc_cab_type);
    if (mb_slave.slave_comm_para.mb_comm_type == 0)
    {
        zucInnerDbg("[Modbus-Slave]:>>Start Init operation-->init modbus-RTU!\n");
        int init_ret = mb_slave_rtu_init(mb_slave.slave_comm_para.deviceName,
                                         mb_slave.slave_comm_para.rtu_comm_para.baudrate,
                                         mb_slave.slave_comm_para.rtu_comm_para.parity,
                                         mb_slave.slave_comm_para.rtu_comm_para.stop_bit_len,
                                         mb_slave.slave_comm_para.rtu_comm_para.byte_len);

        if ((mb_slave.slave_comm_para.mb_comm_type == 0) && (mb_slave.slave_comm_para.slave_id > 247 || mb_slave.slave_comm_para.slave_id < 0))
        {
            zucInnerDbg("[Modbus-Slave]:>>  slaveId=%d  out of range [0, 247]\n", mb_slave.slave_comm_para.slave_id);
            return -1;
        }

        zucInnerDbg("[Modbus-Slave]:>>modbus-RTU Init operation result:%d\n", init_ret);
        return 0;
    }
    else if (mb_slave.slave_comm_para.mb_comm_type == 1)
    {
        zucInnerDbg("[Modbus-Slave]:>>Start Init operation-->Init modbus-TCP!\n");
        mb_slave_tcp_init();

        /* Modbus_Application_Protocol_V1_1b.pdf 
		 * Chapter 4 Section 1 Page 5
		 * RS232/RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
		 */
        query = (uint8_t*)malloc(MODBUS_TCP_MAX_ADU_LENGTH);

        mbsrv2hal_datamappingInit();

        return 0;
    }
    else
    {
        zucInnerDbg("[Modbus-Slave]:>>Start Init operation--> ERROR on communication type!\n");
        return -1;
    }
}

/**
 * @brief deinit interface to release device
 * 
 */
void mb_slave_rtu_DeInit(void)
{
    if (query != NULL)
    {
        free(query);
        query = NULL;
    }
    if (ctx != NULL)
    {
        modbus_free(ctx);
        ctx = NULL;
    }
    if (mb_slave.slave_reg_mapping)
    {
        modbus_mapping_free(mb_slave.slave_reg_mapping);
        mb_slave.slave_reg_mapping = NULL;
    }
} /* End MBIF_modbusDeInit */
/**
 * @brief 
 * 
 */
void mb_slave_tcp_DeInit(void)
{
    if (s >= 0)
    {
        modbus_close(ctx);
    }

    if (ctx != NULL)
    {
        modbus_free(ctx);
        ctx = NULL;
    }
} /* End MBIF_modbusDeInit */
/**
 * @brief 
 * 
 */
void mb_slave_DeInit(void)
{
    if (mb_slave.slave_comm_para.mb_comm_type == 0)
    {
        mb_slave_rtu_DeInit();
    }
    else
    {
        mb_slave_tcp_DeInit();
        if (query != NULL)
        {
            free(query);
            query = NULL;
        }
        if (mb_slave.slave_reg_mapping)
        {
            modbus_mapping_free(mb_slave.slave_reg_mapping);
            mb_slave.slave_reg_mapping = NULL;
        }
    }
}
/**
 * @brief polling the command from master or client
 * 
 * @return char 
 */
char mb_slave_Poll(void)
{
    int rc;
    /*pending the control flow here in the modbus Poll mudule
	  using select method
	  main process will not be suspended*/
    memset(query, 0, MODBUS_RTU_MAX_ADU_LENGTH);
    *((uint32_t*)query) = 0xFFFF;        // 2Byte[index]  2Byte[protocol]
    *((uint32_t*)(query + 4)) = 0xFFFF;  // 2Byte[DataLen]  1Byte[SlaveID]  1Byte[FunctionCode]
    do {
        rc = modbus_receive(ctx, query);
    } while (rc == 0);

    if (rc == -1)
    {
        /* Connection closed by the client or error */
        if (mb_slave.slave_comm_para.mb_comm_type == 1)
        {
            zucInnerDbg("[Modbus-Slave]:>>Poll modbus-TCP ERROR flush data!\n");
        }
        else
        {
            static int errReported = 0;
            if (errReported == 0)
            {
                zucInnerDbg("[Modbus-Slave]:>>Poll modbus-RTU ERROR flush data!\n");
                errReported = 1;
            }
        }
        if (*((uint32_t*)query) != 0xFFFF || *((uint32_t*)(query + 4)) != 0xFFFF)  // 有接收到数据但是接收到的数据不完整
        {
            if (query[modbus_get_header_length(ctx)] == 0)
            {
                modbus_reply_exception(ctx, query, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
            }
            else
            {
                zucInnerDbg("[Modbus-Slave]: invalid data value, Reply_Exception: MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE\n");
                modbus_reply_exception(ctx, query, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
            }

            modbus_flush(ctx);
            return 0;
        }
        else
        {
            modbus_flush(ctx);
            return -1;
        }
    }
    else
    {
    }

    /* data analysis and status update*/
    if (mb_slave_UpdateLocalData() != 0)
    {
        return 0;
    }

    /*according to the request ,send the response back */
    rc = modbus_reply(ctx, query, rc, mb_slave.slave_reg_mapping);
    if (rc == -1)
    {
        zucInnerDbg("Send reply Error\n");
        return MODBUS_POLL_ERROR_SND;
    }
    return 0;
} /* End MBIF_ModbusPoll */
/*0:init
  1:NORMAL
*/
static int rtu_thread_status = 0;
// static unsigned char  mbsrv2hal_server_status;
/**
 * @brief 
 * 
 * @param p 
 * @return void* 
 */
void mb_slave_ServerThread()
{
    char pollstatus;
    // int accept_status;

    //
    int master_socket;
    fd_set refset;
    fd_set rdset;
    /* max file descriptor number */
    int fdmax = 0;
    /* set timeout */
    struct timeval timeout;

    //*(*(mb_srv_tx->HOLDINGREG)) = mbsrv2hal_getModbusSlaveAddr();
    zucInnerDbg("[Modbus-Slave]:>>Performe init operation\n");
    mb_slave_init();
    rtu_thread_status = 1;
    if (mb_slave.slave_comm_para.mb_comm_type == 1)
    {
        server_socket = modbus_tcp_listen(ctx, NB_CONNECTION);
        if (server_socket == -1)
        {
            zucInnerDbg("Unable to listen TCP connection\n");
            zucLogInfo(0, "[Modbus-Slave]:>> Unable to listen TCP connection!");
            modbus_free(ctx);
            return;
        }
        /* Clear the reference set of socket */
        FD_ZERO(&refset);
        /* Add the server socket */
        FD_SET(server_socket, &refset);

        /* Keep track of the max file descriptor */
        fdmax = server_socket;
    }

    while (!quit_flag)
    {
        timeout.tv_sec = 0;
        timeout.tv_usec = 100 * 1000;
        rdset = refset;
        //zucInnerDbg("mb_slave.slave_comm_para.mb_comm_type: is: %d\n", mb_slave.slave_comm_para.mb_comm_type);
        if (mb_slave.slave_comm_para.mb_comm_type == 1)
        {
            if (select(fdmax + 1, &rdset, NULL, NULL, &timeout) == -1)
            {
                zucInnerDbg("server select() failure.\n");
                zucLogInfo(0, "server select() failure!");
                close_sigint(1);
            }

            /* Run through the existing connections looking for data to be
			* read */
            for (master_socket = 0; master_socket <= fdmax; master_socket++)
            {
                if (!FD_ISSET(master_socket, &rdset))
                {
                    continue;
                }

                if (master_socket == server_socket)
                {
                    /* A client is asking a new connection */
                    socklen_t addrlen;
                    struct sockaddr_in clientaddr;
                    int newfd;

                    /* Handle new connections */
                    addrlen = sizeof(clientaddr);
                    memset(&clientaddr, 0, sizeof(clientaddr));
                    newfd = accept(server_socket, (struct sockaddr*)&clientaddr, &addrlen);
                    if (newfd == -1)
                    {
                        perror("Server accept() error");
                    }
                    else
                    {
                        make_nonblocking(newfd);  //FIX ME:maybe unsafe
                        FD_SET(newfd, &refset);

                        if (newfd > fdmax)
                        {
                            /* keep track of the maximum */
                            fdmax = newfd;
                        }
                        zucInnerDbg("New connection from %s:%d on socket %d\n", inet_ntoa(clientaddr.sin_addr), clientaddr.sin_port, newfd);
                    }
                }
                else
                {
                    modbus_set_socket(ctx, master_socket);
                    pollstatus = mb_slave_Poll();
                    if (pollstatus < 0)
                    {
                        zucInnerDbg("[Modbus-Slave]:>> modbus-TCP connection is down: error number is: %d\n", errno);
                        if (errno == 104)
                        {
                            zucInnerDbg("[Modbus-Slave]:>> modbus-TCP connection break by master, try again please!\n");
                        }
                        else if (errno == 11)
                        {
                            zucInnerDbg("[Modbus-Slave]:>> modbus-TCP connection break for hard wire fault, check net connection please!\n");
                        }

                        /*  server in ended on connection closing or any errors. */
                        zucInnerDbg("Connection closed on socket %d\n", master_socket);
                        close(master_socket);

                        /* remove from reference set */
                        FD_CLR(master_socket, &refset);

                        if (master_socket == fdmax)
                        {
                            fdmax--;
                        }
                    }
                }
            }
        }
        else
        {
            pollstatus = mb_slave_Poll();
            if (-1 == pollstatus)
            {
                if (rtu_thread_status == 1)
                {
                    // zucInnerDbg("[Modbus-Slave]:>> modbus-RTU connection is down: error number is: %d\n", errno);
                }
            }
            else
            {
                rtu_thread_status = 1;
            }
        }
    }

    zucInnerDbg("[Modbus-Slave]:>> Modbus server (slave) quit operation\n");
    mb_slave_DeInit();
    return;
}
/* End of mb_slaveServerThread */

/**************************************************************************************
 *Description:
 *input:
 *return:
 ***************************************************************************************/
static int mb_slave_UpdateLocalData(void)
{
    /* Get data form local control box,
	 * the data mainly is the input coils and input registers
	 * writing coils and registers status also update*/
    unsigned short registers_address = 0;
    unsigned short coils_address = 0;
    unsigned short registers_cnt = 0;
    unsigned short coils_cnt = 0;
    unsigned short data_bytes = 0;
    unsigned short byte_length = 0;
    unsigned char modbus_rtu_tcp_offset;

    if (mb_slave.slave_comm_para.mb_comm_type == 1)
    {
        modbus_rtu_tcp_offset = MODBUS_QUERY_TCP_OFFSET;
    }

    if (mb_slave.slave_comm_para.mb_comm_type == 0)
    {
        modbus_rtu_tcp_offset = MODBUS_QUERY_RTU_OFFSET;
    }
    /* Address check*/
    if ((query[MODBUS_QUERY_ID_INDEX + modbus_rtu_tcp_offset] == mb_slave.slave_comm_para.slave_id && mb_slave.slave_comm_para.mb_comm_type == 0) ||
        mb_slave.slave_comm_para.mb_comm_type == 1)
    {
        switch (query[MODBUS_QUERY_FC_INDEX + modbus_rtu_tcp_offset])
        {
        case MODBUS_FC_READ_COILS:
            coils_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            coils_cnt = (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_readcoils(coils_address, coils_cnt);
            break;
        case MODBUS_FC_READ_DISCRETE_INPUTS:
            coils_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            coils_cnt = (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_readDiscretInputs(coils_address, coils_cnt);
            break;
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            registers_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            registers_cnt =
                (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_readHoldingRegisters(registers_address, registers_cnt);
            break;
        case MODBUS_FC_READ_INPUT_REGISTERS:
            registers_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            registers_cnt =
                (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_readInputRegisters(registers_address, registers_cnt);
            break;
        case MODBUS_FC_WRITE_SINGLE_COIL:
            coils_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            data_bytes = (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_WriteSinglcoil(coils_address, data_bytes);
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            registers_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            data_bytes = (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_WriteSinglRegister(registers_address, data_bytes);
            break;
        case MODBUS_FC_WRITE_MULTIPLE_COILS:
            coils_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            coils_cnt = (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            byte_length = query[6 + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_Writecoils(coils_address, coils_cnt, byte_length, &query[7 + modbus_rtu_tcp_offset]);
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            registers_address =
                (unsigned short)query[MODBUS_QUERY_ADDR_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_ADDR_L_INDEX + modbus_rtu_tcp_offset];
            registers_cnt =
                (unsigned short)query[MODBUS_QUERY_LEN_H_INDEX + modbus_rtu_tcp_offset] << 8 | query[MODBUS_QUERY_LEN_L_INDEX + modbus_rtu_tcp_offset];
            byte_length = query[6 + modbus_rtu_tcp_offset];
            mbsrv2hal_update_pinstatus_WriteRegisters(registers_address, registers_cnt, byte_length, &query[7 + modbus_rtu_tcp_offset]);
            break;
        default:
            //返回异常码： 不支持的功能码
            zucInnerDbg("[Modbus-Slave:] updateLocalData(). unsurported functionCode %d\n", query[MODBUS_QUERY_FC_INDEX + modbus_rtu_tcp_offset]);
            modbus_reply_exception(ctx, query, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
            modbus_flush(ctx);
            return -1;
            break;
        }
    }
    else
    {
    }

    return 0;
    /* holding rigisters */
    /* input registers */
} /* End mb_slaveUpdateLocalData */

/**
 * @brief configure file analysis
 * 
 */
static void mbsrv2hal_getModbusSlavePara(mb_slave_comm_para_t& slaveCommuConfig, int cabType)
{
    Util::IniParser ini(ZUC_USER_CONFIG_FILE(usersettings.ini));
    ini.get("MODBUSSLAVE.COMMTYPE", slaveCommuConfig.mb_comm_type, 1, -1, 2);
    ini.get("MODBUSSLAVE.SLAVEID", slaveCommuConfig.slave_id, 1, 1, 247);

    switch (cabType)
    {
    case 1:  //cabv1
        strncpy(slaveCommuConfig.deviceName, "/dev/ttyS3", sizeof(slaveCommuConfig.deviceName));
        break;
    case 2:  //cabv2
    case 3:  //minicab
        strncpy(slaveCommuConfig.deviceName, "/dev/ttyS1", sizeof(slaveCommuConfig.deviceName));
        break;
    default:
        // ini.get("MODBUSSLAVE.DEVNAME", slaveCommuConfig.deviceName, "/dev/ttyS1");
        break;
    }

    //RTU
    ini.get("MODBUSSLAVE.BAUDRATE", slaveCommuConfig.rtu_comm_para.baudrate, 9600, 9600 - 1, 115200 + 1);
    ini.get("MODBUSSLAVE.DATABIT", slaveCommuConfig.rtu_comm_para.byte_len, 8, 5 - 1, 8 + 1);
    ini.get("MODBUSSLAVE.PARITY", slaveCommuConfig.rtu_comm_para.parity, 0, -1, 3);
    ini.get("MODBUSSLAVE.STOPBIT", slaveCommuConfig.rtu_comm_para.stop_bit_len, 1, 0, 3);

    //TCP
    strncpy(slaveCommuConfig.tcp_comm_para.ip_addr, "0.0.0.0", sizeof(slaveCommuConfig.tcp_comm_para.ip_addr));
    ini.get("MODBUSSLAVE.PORT", slaveCommuConfig.tcp_comm_para.port, 6502, 0, 65536);
    zlog()->info("[Modbus-Slave] {} read config: CommType:{} SlaveId:{}\nbaudrate:{} databit:{} parity:{} stopbit:{}\nip:{} port:{}",
                 slaveCommuConfig.deviceName,
                 slaveCommuConfig.mb_comm_type,
                 slaveCommuConfig.slave_id,
                 slaveCommuConfig.rtu_comm_para.baudrate,
                 slaveCommuConfig.rtu_comm_para.byte_len,
                 slaveCommuConfig.rtu_comm_para.parity,
                 slaveCommuConfig.rtu_comm_para.stop_bit_len,
                 slaveCommuConfig.tcp_comm_para.ip_addr,
                 slaveCommuConfig.tcp_comm_para.port);
}

/* End of file */
